#include <errno.h>
#include <stddef.h>
#include <zephyr/kernel.h>
#include <zephyr/sys/printk.h>
#include <zephyr/types.h>
// #include <zephyr/sys/mempool.h>
#include <zephyr/logging/log.h>
#include <zephyr/sys/byteorder.h>
#include <zephyr/sys/slist.h>

#include <zephyr/bluetooth/bluetooth.h>
#include <zephyr/bluetooth/conn.h>
#include <zephyr/bluetooth/gatt.h>
#include <zephyr/bluetooth/hci.h>
#include <zephyr/bluetooth/uuid.h>
#include <zephyr/sys/byteorder.h>

#include "central_multiconn_client.h"

LOG_MODULE_REGISTER(multiconn_client, LOG_LEVEL_DBG);

// Calculate pool sizes based on Kconfig. Add 1 for safety margin? Maybe not
// necessary.
#define MAX_SERVICES \
    (CONFIG_BT_MAX_CONN * CONFIG_MULTICONN_CLIENT_MAX_SERVICES_PER_CONN)
#define MAX_CHARS (MAX_SERVICES * CONFIG_MULTICONN_CLIENT_MAX_CHARS_PER_SERVICE)

// Define memory slabs for dynamic allocation of discovered items
K_MEM_SLAB_DEFINE(svc_slab, sizeof(multiconn_service_t), MAX_SERVICES,
                  __alignof__(multiconn_service_t));
K_MEM_SLAB_DEFINE(char_slab, sizeof(multiconn_characteristic_t), MAX_CHARS,
                  __alignof__(multiconn_characteristic_t));

#ifdef CONFIG_MULTICONN_CLIENT_DISCOVER_DESCRIPTORS_ENABLE
#define MAX_DESCS (MAX_CHARS * CONFIG_MULTICONN_CLIENT_MAX_DESCS_PER_CHAR)
K_MEM_SLAB_DEFINE(desc_slab, sizeof(multiconn_descriptor_t), MAX_DESCS,
                  __alignof__(multiconn_descriptor_t));
#endif

// Pool of connection contexts, one for each possible connection
static struct multiconn_conn_context conn_contexts[CONFIG_BT_MAX_CONN];

static const multiconn_client_callbacks_t *app_callbacks;
static struct bt_conn_cb conn_callbacks;

// UUID Filters
static const struct bt_uuid *const *filter_svc_uuids;
static size_t filter_svc_count;
static const struct bt_uuid *const *filter_char_uuids;
static size_t filter_char_count;
static bool filter_enabled = false;

#if defined(CONFIG_MULTICONN_CLIENT_AUTO_ENABLE_CCC)
// Storage for CCC value to write (static to be accessible by write params)
static uint16_t ccc_value;
// List of UUIDs to auto-enable
static const struct bt_uuid *const *auto_enable_char_uuids;
static size_t auto_enable_char_count;
// Iterator for the auto-enable list during ENABLING_CCC state
static size_t current_auto_enable_index;
#endif

static struct multiconn_conn_context *find_context(struct bt_conn *conn);
static struct multiconn_conn_context *find_free_context(void);
static void cleanup_context_data(struct multiconn_conn_context *ctx);
static void schedule_next_action(struct multiconn_conn_context *ctx);
static void mcc_work_handler(struct k_work *work);

static void mcc_connected(struct bt_conn *conn, uint8_t err);
static void mcc_disconnected(struct bt_conn *conn, uint8_t reason);
#ifdef CONFIG_MULTICONN_CLIENT_CONN_PARAM_UPDATE_ENABLE
static void mcc_le_param_updated(struct bt_conn *conn, uint16_t interval,
                                 uint16_t latency, uint16_t timeout);
#endif

/* GATT Callback Implementations */
static void mcc_mtu_exchange_cb(struct bt_conn *conn, uint8_t err,
                                struct bt_gatt_exchange_params *params);
static uint8_t mcc_discover_service_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr,
                                       struct bt_gatt_discover_params *params);
static uint8_t mcc_discover_characteristic_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr, struct bt_gatt_discover_params *params);
#ifdef CONFIG_MULTICONN_CLIENT_DISCOVER_DESCRIPTORS_ENABLE
static uint8_t mcc_discover_descriptor_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr,
                                          struct bt_gatt_discover_params *params);
#endif
#if defined(CONFIG_MULTICONN_CLIENT_AUTO_ENABLE_CCC)
static void mcc_write_ccc_cb(struct bt_conn *conn, uint8_t err, struct bt_gatt_write_params *params);
#endif

/* Filter Check Helpers */
static bool should_discover_service(const struct bt_uuid *uuid);
static bool should_discover_characteristic(const struct bt_uuid *uuid);

static uint8_t bt_uuid_get_len(const struct bt_uuid *uuid)
{
    if (!uuid)
    {
        return 0;
    }

    switch (uuid->type)
    {
        case BT_UUID_TYPE_16:
            return 2;

        case BT_UUID_TYPE_32:
            return 4;

        case BT_UUID_TYPE_128:
            return 16;

        default:
            return 0;
    }
}

static struct multiconn_conn_context *find_context(struct bt_conn *conn)
{
    if (!conn)
    {
        return NULL;
    }
    for (int i = 0; i < CONFIG_BT_MAX_CONN; i++)
    {
        if (conn_contexts[i].in_use && conn_contexts[i].conn == conn)
        {
            return &conn_contexts[i];
        }
    }
    return NULL;
}

static struct multiconn_conn_context *find_free_context(void)
{
    for (int i = 0; i < CONFIG_BT_MAX_CONN; i++)
    {
        if (!conn_contexts[i].in_use)
        {
            // Optionally: Fully zero the context before returning? Resetting in
            // mcc_connected is sufficient.
            return &conn_contexts[i];
        }
    }
    LOG_WRN("No free connection contexts available (max %d).",
            CONFIG_BT_MAX_CONN);
    return NULL;
}

static void cleanup_context_data(struct multiconn_conn_context *ctx)
{
    if (!ctx)
        return;

    sys_snode_t *svc_node, *char_node;
#ifdef CONFIG_MULTICONN_CLIENT_DISCOVER_DESCRIPTORS_ENABLE
    sys_snode_t *desc_node;
#endif

    // Iterate and free all characteristics and descriptors within each service
    while ((svc_node = sys_slist_get(&ctx->services)))
    {
        multiconn_service_t *svc =
            CONTAINER_OF(svc_node, multiconn_service_t, node);
        while ((char_node = sys_slist_get(&svc->characteristics)))
        {
            multiconn_characteristic_t *chara =
                CONTAINER_OF(char_node, multiconn_characteristic_t, node);
#ifdef CONFIG_MULTICONN_CLIENT_DISCOVER_DESCRIPTORS_ENABLE
            while ((desc_node = sys_slist_get(&chara->descriptors)))
            {
                multiconn_descriptor_t *desc =
                    CONTAINER_OF(desc_node, multiconn_descriptor_t, node);
                k_mem_slab_free(&desc_slab, (void *)desc); // Free descriptor
            }
#endif
            k_mem_slab_free(&char_slab, (void *)chara); // Free characteristic
        }
        k_mem_slab_free(&svc_slab, (void *)svc); // Free service
    }
    // Reset counters and ensure list is marked empty
    ctx->service_count = 0;
    // sys_slist_init(&ctx->services); // Not needed after sys_slist_get loop
}

static void schedule_next_action(struct multiconn_conn_context *ctx)
{
    if (!ctx || !ctx->in_use || ctx->conn == NULL ||
        ctx->state == MULTICONN_STATE_DISCONNECTED)
    {
        LOG_WRN("Attempted to schedule action for inactive/invalid context %p",
                (void *)ctx);
        return;
    }
    // Submit the context's work item to the system work queue
    int ret = k_work_submit(&ctx->work);
    if (ret < 0 && ret != -EALREADY)
    {
        // EALREADY (-22) means already queued, which is fine.
        LOG_ERR("Failed to submit work item for conn %p (err %d)",
                (void *)ctx->conn, ret);
        // This is potentially serious, maybe set error state?
        // ctx->last_err = ret;
        // ctx->state = MULTICONN_STATE_ERROR;
        // Consider how to recover or report this.
    }
}

static bool should_discover_service(const struct bt_uuid *uuid)
{
    if (!filter_enabled || filter_svc_count == 0)
    {
        return true; // No filter active or no services specified in filter
    }
    for (size_t i = 0; i < filter_svc_count; i++)
    {
        if (filter_svc_uuids[i] && bt_uuid_cmp(uuid, filter_svc_uuids[i]) == 0)
        {
            return true; // Found in filter list
        }
    }
    return false; // Not in filter list
}

static bool should_discover_characteristic(const struct bt_uuid *uuid)
{
    if (!filter_enabled || filter_char_count == 0)
    {
        return true; // No filter active or no characteristics specified in filter
    }
    for (size_t i = 0; i < filter_char_count; i++)
    {
        if (filter_char_uuids[i] && bt_uuid_cmp(uuid, filter_char_uuids[i]) == 0)
        {
            return true; // Found in filter list
        }
    }
    return false; // Not in filter list
}

static void mcc_connected(struct bt_conn *conn, uint8_t err)
{
    char addr[BT_ADDR_LE_STR_LEN];
    bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

    if (err)
    {
        LOG_ERR("Connection attempt failed to %s (err %u) for conn %p", addr, err, (void *)conn);

        // No context associated yet, nothing to clean up here.
        if (app_callbacks && app_callbacks->error)
        {
            app_callbacks->error(bt_conn_get_dst(conn), err);
        }
        return;
    }

    LOG_INF("Connected event for: %s, conn %p", addr, (void *)conn);

    // --- Robustness Check: Prevent double handling ---
    struct multiconn_conn_context *existing_ctx = find_context(conn);
    if (existing_ctx)
    {
        LOG_WRN("Duplicate 'connected' event for already managed conn %p (state: "
                "%d). Ignoring.",
                (void *)conn, existing_ctx->state);
        return;
    }

    // --- Find and Allocate Context ---
    struct multiconn_conn_context *ctx = find_free_context();
    if (!ctx)
    {
        LOG_ERR("No free context available for new connection %s (max %d). "
                "Disconnecting.",
                addr, CONFIG_BT_MAX_CONN);
        bt_conn_disconnect(conn, BT_HCI_ERR_CONN_LIMIT_EXCEEDED);
        return;
    }

    // --- Initialize New Context ---
    LOG_DBG("Assigning free context %p for conn %p", (void *)ctx, (void *)conn);
    ctx->conn = bt_conn_ref(conn); // Take reference
    ctx->in_use = true;
    ctx->state = MULTICONN_STATE_CONNECTED;
    ctx->last_err = 0;
    sys_slist_init(&ctx->services);
    ctx->service_count = 0;
    ctx->current_service_node = NULL;
    ctx->current_char_node = NULL;

#ifdef CONFIG_MULTICONN_CLIENT_CONN_PARAM_UPDATE_ENABLE
    // Apply desired connection parameters from Kconfig
    ctx->desired_conn_params.interval_min =
        CONFIG_MULTICONN_CLIENT_CONN_PARAM_MIN_INT;
    ctx->desired_conn_params.interval_max =
        CONFIG_MULTICONN_CLIENT_CONN_PARAM_MAX_INT;
    ctx->desired_conn_params.latency = CONFIG_MULTICONN_CLIENT_CONN_PARAM_LATENCY;
    ctx->desired_conn_params.timeout = CONFIG_MULTICONN_CLIENT_CONN_PARAM_TIMEOUT;
#endif

    // Initialize work item associated with this context
    k_work_init(&ctx->work, mcc_work_handler);

    // Schedule the first action (MTU exchange) via the work queue
    schedule_next_action(ctx);
}

/** @brief Handles the 'disconnected' event from the Bluetooth stack. */
static void mcc_disconnected(struct bt_conn *conn, uint8_t reason)
{
    char addr[BT_ADDR_LE_STR_LEN];
    bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
    LOG_INF("Disconnected: %s (reason 0x%02x), conn %p", addr, reason,
            (void *)conn);

    struct multiconn_conn_context *ctx = find_context(conn);
    if (!ctx)
    {
        LOG_WRN("Disconnected context not found for %s, conn %p", addr,
                (void *)conn);
        return;
    }

    // Store reason before potential cleanup changes state? Not strictly necessary
    // here. ctx->disconnection_reason = reason;

    // Call application callback *before* cleaning up context data
    if (app_callbacks && app_callbacks->disconnected)
    {
        app_callbacks->disconnected(ctx, reason);
    }

    // Cleanup dynamically allocated service/char/desc data
    cleanup_context_data(ctx);

    // Reset context state
    ctx->state = MULTICONN_STATE_DISCONNECTED;
    ctx->last_err = 0;
    ctx->in_use = false;
    bt_conn_unref(ctx->conn); // Release reference
    ctx->conn = NULL;

    // Cancel any pending work for this context?
    // k_work_cancel(&ctx->work); // Check Zephyr version for API
    // availability/necessity
}

#ifdef CONFIG_MULTICONN_CLIENT_CONN_PARAM_UPDATE_ENABLE
/** @brief Handles the connection parameter update event. */
static void mcc_le_param_updated(struct bt_conn *conn, uint16_t interval,
                                 uint16_t latency, uint16_t timeout)
{
    struct multiconn_conn_context *ctx = find_context(conn);
    if (!ctx)
    {
        LOG_WRN("Param update CB: Context not found for conn %p", (void *)conn);
        return;
    }

    LOG_INF("Conn params updated for conn %p: interval %u (%.2f ms), latency %u, "
            "timeout %u (%d ms)",
            (void *)conn, interval, (double)(interval * 1.25f), latency, timeout,
            timeout * 10);

    // Check if we were waiting for this update
    if (ctx->state == MULTICONN_STATE_CONN_PARAM_UPDATING)
    {
        // Proceed to the next state (Service Discovery)
        ctx->state = MULTICONN_STATE_SERVICE_DISCOVERING;
        schedule_next_action(ctx);
    }
    else
    {
        LOG_WRN("Param update CB received in unexpected state %d for conn %p",
                ctx->state, (void *)conn);
        // Application might still be interested in the update event, consider a
        // passthrough callback?
    }
}
#endif

static void mcc_mtu_exchange_cb(struct bt_conn *conn, uint8_t err,
                                struct bt_gatt_exchange_params *params)
{
    struct multiconn_conn_context *ctx = find_context(conn);
    if (!ctx)
    {
        LOG_ERR("MTU CB: Context not found for conn %p", (void *)conn);
        return;
    }
    if (ctx->state != MULTICONN_STATE_MTU_EXCHANGING)
    {
        LOG_WRN("MTU CB received in unexpected state %d for conn %p", ctx->state,
                (void *)conn);
        return;
    }

    if (err)
    {
        LOG_ERR("MTU exchange failed for conn %p (err %u)", (void *)conn, err);
        ctx->last_err = err; // Use ATT error code directly
        ctx->state = MULTICONN_STATE_ERROR;
    }
    else
    {
        LOG_INF("MTU updated to %u for conn %p", bt_gatt_get_mtu(conn),
                (void *)conn);
// Proceed to next state
#ifdef CONFIG_MULTICONN_CLIENT_CONN_PARAM_UPDATE_ENABLE
        ctx->state = MULTICONN_STATE_CONN_PARAM_UPDATING;
#else
        ctx->state = MULTICONN_STATE_SERVICE_DISCOVERING;
#endif
    }
    schedule_next_action(ctx);
}

static uint8_t mcc_discover_service_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr,
                                       struct bt_gatt_discover_params *params)
{
    struct multiconn_conn_context *ctx = find_context(conn);
    if (!ctx)
    {
        LOG_ERR("Service Discover CB: Context not found");
        return BT_GATT_ITER_STOP;
    }
    if (ctx->state != MULTICONN_STATE_SERVICE_DISCOVERING)
    {
        LOG_WRN("Service Discover CB in unexpected state %d", ctx->state);
        return BT_GATT_ITER_STOP;
    }

    if (!attr)
    { // Discovery complete or error
        LOG_INF("Service discovery complete for conn %p (found %u matching services)", (void *)conn, ctx->service_count);

        if (ctx->last_err == 0)
        { // No errors encountered during iteration
            if (ctx->service_count > 0)
            {
                ctx->state = MULTICONN_STATE_CHAR_DISCOVERING;
                ctx->current_service_node = sys_slist_peek_head(&ctx->services); // Start char discovery from first service
            }
            else
            {
                LOG_INF("No matching services found for conn %p.", (void *)conn);
                // No services -> no chars -> check auto-enable (unlikely) -> ready
                ctx->state = MULTICONN_STATE_ERROR;
                schedule_next_action(ctx);
                return BT_GATT_ITER_STOP; // Stop discovery process
            }
        }
        else
        { // Error occurred (e.g., memory limit)
            ctx->state = MULTICONN_STATE_ERROR;
        }
        schedule_next_action(ctx); // Schedule next step (char discovery or error/ready)
        return BT_GATT_ITER_STOP;  // Stop discovery process
    }

    // Process discovered primary service
    if (params->type == BT_GATT_DISCOVER_PRIMARY && attr->user_data)
    {
        struct bt_gatt_service_val *svc_val = (struct bt_gatt_service_val *)attr->user_data;
        char uuid_str[BT_UUID_STR_LEN];
        bt_uuid_to_str((const struct bt_uuid *)svc_val->uuid, uuid_str, sizeof(uuid_str));

        // Apply service filter
        if (!should_discover_service(svc_val->uuid))
        {
            LOG_DBG("Skipping filtered service UUID %s", uuid_str);
            return BT_GATT_ITER_CONTINUE;
        }

        // only discover 128bits UUID
        if (svc_val->uuid->type != BT_UUID_TYPE_128)
        {
            LOG_DBG("Skipping 16bits service UUID %s", uuid_str);
            return BT_GATT_ITER_CONTINUE;
        }

        LOG_DBG("Found service: UUID=%s, start=0x%04x, end=0x%04x", uuid_str, attr->handle, svc_val->end_handle);

        // Check limits
        if (ctx->service_count >= CONFIG_MULTICONN_CLIENT_MAX_SERVICES_PER_CONN)
        {
            LOG_WRN("Max services per connection (%d) reached.", CONFIG_MULTICONN_CLIENT_MAX_SERVICES_PER_CONN);
            ctx->last_err = -ENOBUFS;     // Indicate limit reached
            return BT_GATT_ITER_CONTINUE; // Continue search, but don't store
        }

        // Allocate and store service info using memory slab
        multiconn_service_t *new_svc;
        if (k_mem_slab_alloc(&svc_slab, (void **)&new_svc, K_NO_WAIT) != 0)
        {
            LOG_ERR("Failed to allocate memory for service from slab.");
            ctx->last_err = -ENOMEM;
            return BT_GATT_ITER_STOP; // Stop discovery
        }
        memset(new_svc, 0, sizeof(multiconn_service_t));

        // Copy UUID and handle range
        const struct bt_uuid *src_uuid = svc_val->uuid;
        size_t size_to_copy;
        switch (src_uuid->type)
        {
            case BT_UUID_TYPE_16:
                size_to_copy = sizeof(struct bt_uuid_16);
                break;

            case BT_UUID_TYPE_32:
                size_to_copy = sizeof(struct bt_uuid_32); // If BT_UUID_TYPE_32 is supported
                break;

            case BT_UUID_TYPE_128:
                size_to_copy = sizeof(struct bt_uuid_128);
                break;

            default:
                // Handle error: unknown UUID type
                size_to_copy = 0; // Prevent copy
                // Log error, set context error, stop iteration...
                break;
        }
        LOG_DBG("SVC uuid size: %d", size_to_copy);

        if (size_to_copy > 0 && size_to_copy <= sizeof(new_svc->uuid))
        {
            memcpy(&new_svc->uuid, src_uuid, size_to_copy);
            new_svc->start_handle = attr->handle;
            new_svc->end_handle = svc_val->end_handle;
        }
        else
        {
            // Handle unexpected error: source UUID size is too large
            LOG_ERR("Source UUID size (%zu) > destination size (%zu)", size_to_copy, sizeof(struct bt_uuid_128));
            ctx->last_err = -EINVAL;                     // Or another suitable error
            k_mem_slab_free(&svc_slab, (void *)new_svc); // Free allocated memory
            return BT_GATT_ITER_STOP;                    // Stop discovery
        }

        sys_slist_init(&new_svc->characteristics);
        new_svc->char_count = 0;

        sys_slist_append(&ctx->services, &new_svc->node);
        ctx->service_count++;
    }

    return BT_GATT_ITER_CONTINUE; // Continue discovering services
}

static uint8_t mcc_discover_characteristic_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr,
                                              struct bt_gatt_discover_params *params)
{
    struct multiconn_conn_context *ctx = find_context(conn);
    if (!ctx)
    {
        LOG_ERR("Char Discover CB: Context not found");
        return BT_GATT_ITER_STOP;
    }
    if (!ctx->current_service_node)
    {
        LOG_ERR("Char CB: No current service node");
        return BT_GATT_ITER_STOP;
    }
    if (ctx->state != MULTICONN_STATE_CHAR_DISCOVERING)
    {
        LOG_WRN("Char Discover CB in unexpected state %d", ctx->state);
        return BT_GATT_ITER_STOP;
    }
    multiconn_service_t *current_svc = CONTAINER_OF(ctx->current_service_node, multiconn_service_t, node);

    if (!attr)
    {
        // Discovery complete for this service or error
        char svc_uuid_str[BT_UUID_STR_LEN];
        bt_uuid_to_str((const struct bt_uuid *)&current_svc->uuid, svc_uuid_str, sizeof(svc_uuid_str));
        LOG_INF("Characteristic discovery complete for service UUID %s (found %u matching chars)",
                svc_uuid_str, current_svc->char_count);

        if (ctx->last_err != 0)
        { // Error occurred during iteration
            ctx->state = MULTICONN_STATE_ERROR;
            schedule_next_action(ctx);
            return BT_GATT_ITER_STOP;
        }

        // Move to the next service for characteristic discovery
        ctx->current_service_node = sys_slist_peek_next(ctx->current_service_node);
        schedule_next_action(ctx); // Let work handler manage state transition / next discovery
        return BT_GATT_ITER_STOP;  // Stop discovery for *this* service range
    }

    // Process discovered characteristic
    if (params->type == BT_GATT_DISCOVER_CHARACTERISTIC && attr->user_data)
    {
        struct bt_gatt_chrc *chrc_val = (struct bt_gatt_chrc *)attr->user_data;
        char uuid_str[BT_UUID_STR_LEN];
        bt_uuid_to_str((const struct bt_uuid *)chrc_val->uuid, uuid_str, sizeof(uuid_str));

        // Apply characteristic filter
        if (!should_discover_characteristic(chrc_val->uuid))
        {
            LOG_DBG("Skipping filtered characteristic UUID %s", uuid_str);
            return BT_GATT_ITER_CONTINUE;
        }

        LOG_DBG("  Found char: UUID=%s, decl=0x%04x, value=0x%04x, props=0x%02x",
                uuid_str, attr->handle, chrc_val->value_handle, chrc_val->properties);

        // Check limits
        if (current_svc->char_count >= CONFIG_MULTICONN_CLIENT_MAX_CHARS_PER_SERVICE)
        {
            LOG_WRN("Max characteristics per service (%d) reached.", CONFIG_MULTICONN_CLIENT_MAX_CHARS_PER_SERVICE);
            ctx->last_err = -ENOBUFS;
            return BT_GATT_ITER_CONTINUE; // Continue search, don't store
        }

        // Allocate and store characteristic info using memory slab
        multiconn_characteristic_t *new_char;
        if (k_mem_slab_alloc(&char_slab, (void **)&new_char, K_NO_WAIT) != 0)
        {
            LOG_ERR("Failed to allocate memory for characteristic from slab.");
            ctx->last_err = -ENOMEM;
            return BT_GATT_ITER_STOP; // Stop discovery
        }
        memset(new_char, 0, sizeof(multiconn_characteristic_t));

        // Copy data
        const struct bt_uuid *chrc_uuid = chrc_val->uuid;
        size_t size_to_copy;
        switch (chrc_uuid->type)
        {
            case BT_UUID_TYPE_16:
                size_to_copy = sizeof(struct bt_uuid_16);
                break;

            case BT_UUID_TYPE_32:
                size_to_copy = sizeof(struct bt_uuid_32); // If BT_UUID_TYPE_32 is supported
                break;

            case BT_UUID_TYPE_128:
                size_to_copy = sizeof(struct bt_uuid_128);
                break;

            default:
                // Handle error: unknown UUID type
                size_to_copy = 0; // Prevent copy
                // Log error, set context error, stop iteration...
                break;
        }
        LOG_DBG("CHRC uuid size: %d", size_to_copy);

        if (size_to_copy > 0 && size_to_copy <= sizeof(new_char->uuid))
        {
            memcpy(&new_char->uuid, chrc_uuid, size_to_copy);
            new_char->decl_handle = attr->handle;
            new_char->value_handle = chrc_val->value_handle;
            new_char->properties = chrc_val->properties;
        }
        else
        {
            // Handle unexpected error: source UUID size is too large
            LOG_ERR("Source UUID size (%zu) > destination size (%zu)", size_to_copy, sizeof(struct bt_uuid_128));
            ctx->last_err = -EINVAL;                       // Or another suitable error
            k_mem_slab_free(&char_slab, (void *)new_char); // Free allocated memory
            return BT_GATT_ITER_STOP;                      // Stop discovery
        }
        // bt_uuid_create(&new_char->uuid, chrc_val->uuid->data, bt_uuid_get_len(chrc_val->uuid));
        // new_char->decl_handle = attr->handle;
        // new_char->value_handle = chrc_val->value_handle;
        // new_char->properties = chrc_val->properties;

        // Initialize subscribe parameters (app needs to manage actual subscription state)
        // This assumes notifications will be used. Adjust if indications are primary.
        // The actual value handle needs to be set before subscribing.
        // new_char->sub_params.value_handle = new_char->value_handle; // Do this here or before subscribe?
        // new_char->sub_params.notify = mcc_notify_cb; // App should set its own notify CB
        // new_char->sub_params.value = 1; // Enable notifications
        // new_char->sub_params.ccc_handle = ?? // Need to find this first if descriptors enabled

#ifdef CONFIG_MULTICONN_CLIENT_DISCOVER_DESCRIPTORS_ENABLE
        sys_slist_init(&new_char->descriptors);
        new_char->desc_count = 0;
        new_char->ccc_handle = 0; // Will be updated if CCCD is found
#endif

        sys_slist_append(&current_svc->characteristics, &new_char->node);
        current_svc->char_count++;
    }

    return BT_GATT_ITER_CONTINUE; // Continue discovering characteristics in this range
}

#ifdef CONFIG_MULTICONN_CLIENT_DISCOVER_DESCRIPTORS_ENABLE
/** @brief Handles descriptor discovery results. */
static uint8_t mcc_discover_descriptor_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr,
                                          struct bt_gatt_discover_params *params)
{
    struct multiconn_conn_context *ctx = find_context(conn);
    if (!ctx)
    {
        LOG_ERR("Desc CB: Context not found");
        return BT_GATT_ITER_STOP;
    }
    if (!ctx->current_service_node || !ctx->current_char_node)
    {
        LOG_ERR("Desc CB: Missing current service/char node");
        return BT_GATT_ITER_STOP;
    }
    if (ctx->state != MULTICONN_STATE_DESC_DISCOVERING)
    {
        LOG_WRN("Desc Discover CB in unexpected state %d", ctx->state);
        return BT_GATT_ITER_STOP;
    }
    multiconn_characteristic_t *current_char = CONTAINER_OF(ctx->current_char_node, multiconn_characteristic_t, node);

    if (!attr)
    { // Discovery complete for this characteristic or error
        char char_uuid_str[BT_UUID_STR_LEN];
        bt_uuid_to_str((const struct bt_uuid *)&current_char->uuid, char_uuid_str, sizeof(char_uuid_str));
        LOG_DBG("Descriptor discovery complete for char UUID %s (found %u descs)",
                char_uuid_str, current_char->desc_count);

        if (ctx->last_err != 0)
        { // Error occurred
            ctx->state = MULTICONN_STATE_ERROR;
        }
        else if (ctx->state != MULTICONN_STATE_ERROR)
        {
            ctx->current_char_node = sys_slist_peek_next(ctx->current_char_node);
            if (ctx->current_char_node == NULL)
            {
                // Reached end of characteristics for this service
                ctx->current_service_node = sys_slist_peek_next(ctx->current_service_node);
            }
        }
        schedule_next_action(ctx);
        return BT_GATT_ITER_STOP; // Stop discovery for *this* characteristic's range
    }

    // Process discovered descriptor
    if (params->type == BT_GATT_DISCOVER_DESCRIPTOR && attr->uuid)
    {
        const struct bt_uuid *desc_uuid = attr->uuid;
        char uuid_str[BT_UUID_STR_LEN];
        bt_uuid_to_str((const struct bt_uuid *)desc_uuid, uuid_str, sizeof(uuid_str));
        LOG_DBG("    Found descriptor: UUID=%s, handle=0x%04x", uuid_str, attr->handle);

        // Check limits
        if (current_char->desc_count >= CONFIG_MULTICONN_CLIENT_MAX_DESCS_PER_CHAR)
        {
            LOG_WRN("Max descriptors per characteristic (%d) reached.", CONFIG_MULTICONN_CLIENT_MAX_DESCS_PER_CHAR);
            ctx->last_err = -ENOBUFS;
            return BT_GATT_ITER_CONTINUE; // Continue search, don't store
        }

        // Allocate and store descriptor info using memory slab
        multiconn_descriptor_t *new_desc;
        if (k_mem_slab_alloc(&desc_slab, (void **)&new_desc, K_NO_WAIT) != 0)
        {
            LOG_ERR("Failed to allocate memory for descriptor from slab.");
            ctx->last_err = -ENOMEM;
            return BT_GATT_ITER_STOP; // Stop discovery
        }
        memset(new_desc, 0, sizeof(multiconn_descriptor_t));

        // Copy data
        size_t size_to_copy;
        switch (desc_uuid->type)
        {
            case BT_UUID_TYPE_16:
                size_to_copy = sizeof(struct bt_uuid_16);
                memcpy((struct bt_uuid_16 *)&new_desc->uuid, (struct bt_uuid_16 *)desc_uuid, size_to_copy);
                break;

            case BT_UUID_TYPE_32:
                size_to_copy = sizeof(struct bt_uuid_32); // If BT_UUID_TYPE_32 is supported
                memcpy((struct bt_uuid_32 *)&new_desc->uuid, (struct bt_uuid_32 *)desc_uuid, size_to_copy);
                break;

            case BT_UUID_TYPE_128:
                size_to_copy = sizeof(struct bt_uuid_128);
                memcpy((struct bt_uuid_128 *)&new_desc->uuid, (struct bt_uuid_128 *)desc_uuid, size_to_copy);
                break;

            default:
                // Handle error: unknown UUID type
                size_to_copy = 0; // Prevent copy
                // Log error, set context error, stop iteration...
                break;
        }
        LOG_DBG("DESC uuid size: %d", size_to_copy);

        new_desc->handle = attr->handle;

        sys_slist_append(&current_char->descriptors, &new_desc->node);
        current_char->desc_count++;

        // Check if it's the CCCD and store handle shortcut
        if (bt_uuid_cmp((const struct bt_uuid *)&new_desc->uuid, BT_UUID_GATT_CCC) == 0)
        {
            current_char->ccc_handle = new_desc->handle;
            LOG_DBG("    -> CCCD Handle stored: 0x%04x", new_desc->handle);
            // Update subscribe params if needed?
            // current_char->sub_params.ccc_handle = new_desc->handle;

#if defined(CONFIG_MULTICONN_CLIENT_AUTO_ENABLE_CCC)
            bool is_notify = (current_char->properties & BT_GATT_CHRC_NOTIFY);
            bool is_indicate = (current_char->properties & BT_GATT_CHRC_INDICATE);
            if ((is_notify || is_indicate) && current_char->ccc_handle != 0)
            {
                // Prepare and start CCCD write
                ccc_value = is_notify ? BT_GATT_CCC_NOTIFY : BT_GATT_CCC_INDICATE;
                printk("ccc_value: %d\n", ccc_value);
                ctx->write_params.func = mcc_write_ccc_cb;
                ctx->write_params.handle = current_char->ccc_handle;
                ctx->write_params.offset = 0;
                ctx->write_params.data = &ccc_value;
                ctx->write_params.length = sizeof(ccc_value);

                int err = bt_gatt_write(ctx->conn, &ctx->write_params);
                if (err)
                {
                    char uuid_str[BT_UUID_STR_LEN];
                    bt_uuid_to_str((const struct bt_uuid *)&current_char->uuid, uuid_str, sizeof(uuid_str));
                    LOG_ERR("Failed start CCC write for target UUID %s (handle 0x%04x) conn %p (err %d)",
                            uuid_str, current_char->ccc_handle, (void *)ctx->conn, err);
                    // Don't set fatal error, just log and move to next UUID in list
                }
                else
                {
                    char uuid_str[BT_UUID_STR_LEN];
                    bt_uuid_to_str((const struct bt_uuid *)&current_char->uuid, uuid_str, sizeof(uuid_str));
                    LOG_INF("Started CCC write for target UUID %s (handle 0x%04x) conn %p",
                            uuid_str, current_char->ccc_handle, (void *)ctx->conn);
                    // Wait for mcc_write_ccc_cb callback
                    // return; // Exit work handler
                }
            }
            else
            {
                char uuid_str[BT_UUID_STR_LEN];
                bt_uuid_to_str((const struct bt_uuid *)&current_char->uuid, uuid_str, sizeof(uuid_str));
                LOG_WRN("Target UUID %s found, but not Notify/Indicate or CCC Handle missing (props=0x%x, ccc=0x%x)",
                        uuid_str, current_char->properties, current_char->ccc_handle);
            }
#endif
        }
    }

    return BT_GATT_ITER_CONTINUE; // Continue discovering descriptors in this range
}
#endif // CONFIG_MULTICONN_CLIENT_DISCOVER_DESCRIPTORS_ENABLE

#if defined(CONFIG_MULTICONN_CLIENT_AUTO_ENABLE_CCC)
/** @brief Handles GATT write completion for CCCD enabling. */
static void mcc_write_ccc_cb(struct bt_conn *conn, uint8_t err, struct bt_gatt_write_params *params)
{
    struct multiconn_conn_context *ctx = find_context(conn);
    if (!ctx)
    {
        LOG_ERR("CCC Write CB: Context not found for conn %p", (void *)conn);
        return;
    }
    // if (ctx->state != MULTICONN_STATE_ENABLING_CCC)
    // {
    //     LOG_WRN("CCC Write CB received in unexpected state %d for conn %p", ctx->state, (void *)conn);
    //     return;
    // }

    // Log result
    if (err)
    {
        LOG_ERR("Failed to write CCCD at handle 0x%04x for conn %p (err %u)", params->handle, (void *)conn, err);
        // Consider if this should set ctx->last_err? Usually no, we just continue.
    }
    else
    {
        LOG_INF("Successfully wrote CCCD at handle 0x%04x for conn %p", params->handle, (void *)conn);
    }

    // Writing finished (success or fail), let the work handler find the next UUID in the list.
    // The work handler is responsible for incrementing current_auto_enable_index.
    // schedule_next_action(ctx);
}
#endif // CONFIG_MULTICONN_CLIENT_AUTO_ENABLE_CCC

int multiconn_client_init(const multiconn_client_callbacks_t *callbacks)
{
    if (!callbacks || !callbacks->ready || !callbacks->disconnected)
    {
        LOG_ERR(
            "Invalid callbacks provided (ready and disconnected are required).");
        return -EINVAL;
    }
    app_callbacks = callbacks;

    // 初始化上下文信息
    memset(conn_contexts, 0, sizeof(conn_contexts));
    for (int i = 0; i < CONFIG_BT_MAX_CONN; ++i)
    {
        conn_contexts[i].in_use = false;
        // Work items are initialized in mcc_connected when context is assigned
    }

    conn_callbacks.connected = mcc_connected;
    conn_callbacks.disconnected = mcc_disconnected;
#ifdef CONFIG_MULTICONN_CLIENT_CONN_PARAM_UPDATE_ENABLE
    conn_callbacks.le_param_updated = mcc_le_param_updated;
#endif

    bt_conn_cb_register(&conn_callbacks);

    LOG_INF("Multi-connection client library initialized (Max Conns: %d).",
            CONFIG_BT_MAX_CONN);
    LOG_INF("Service/Char/Desc Limits: %d/%d/%d",
            CONFIG_MULTICONN_CLIENT_MAX_SERVICES_PER_CONN,
            CONFIG_MULTICONN_CLIENT_MAX_CHARS_PER_SERVICE,
            0);
    LOG_INF("Conn Update: %s, Desc Discovery: %s, Auto CCC: %s",
            IS_ENABLED(CONFIG_MULTICONN_CLIENT_CONN_PARAM_UPDATE_ENABLE)
                ? "Enabled"
                : "Disabled",
            "Disabled",
            IS_ENABLED(CONFIG_MULTICONN_CLIENT_AUTO_ENABLE_CCC) ? "Enabled"
                                                                : "Disabled");

    return 0;
}

int multiconn_client_set_filter(const struct bt_uuid *const *svc_uuids, size_t svc_count,
                                const struct bt_uuid *const *char_uuids, size_t char_count)
{
    // Basic validation
    if ((svc_count > 0 && !svc_uuids) || (char_count > 0 && !char_uuids))
    {
        return -EINVAL;
    }

    // Store pointers, assuming they remain valid
    filter_svc_uuids = svc_uuids;
    filter_svc_count = svc_count;
    filter_char_uuids = char_uuids;
    filter_char_count = char_count;
    filter_enabled = (svc_count > 0 || char_count > 0);

    LOG_INF("UUID Filter %s: Services=%zu, Characteristics=%zu",
            filter_enabled ? "ENABLED" : "DISABLED", svc_count, char_count);

    return 0;
}

int multiconn_client_set_auto_enable_list(const struct bt_uuid *const *uuids, size_t count)
{
#if defined(CONFIG_MULTICONN_CLIENT_AUTO_ENABLE_CCC)
    if (count > 0 && !uuids)
    {
        return -EINVAL;
    }
    auto_enable_char_uuids = uuids;
    auto_enable_char_count = count;
    LOG_INF("Auto-enable CCC list set with %zu UUIDs.", count);
    return 0;
#else
    LOG_WRN("Auto-enable CCC feature is disabled via Kconfig. Call has no effect.");
    return -ENOTSUP;
#endif
}

struct multiconn_conn_context *multiconn_client_get_context(struct bt_conn *conn)
{
    // Simply call the internal helper
    return find_context(conn);
}

multiconn_characteristic_t *multiconn_client_find_characteristic(multiconn_service_t *svc,
                                                                 const struct bt_uuid *uuid)
{
    if (!svc || !uuid)
        return NULL;
    sys_snode_t *char_node;
    SYS_SLIST_FOR_EACH_NODE(&svc->characteristics, char_node)
    {
        multiconn_characteristic_t *chara = CONTAINER_OF(char_node, multiconn_characteristic_t, node);
        if (bt_uuid_cmp((const struct bt_uuid *)&chara->uuid, uuid) == 0)
        {
            return chara;
        }
    }
    return NULL; // Not found
}

multiconn_characteristic_t *multiconn_client_find_characteristic_with_props(multiconn_service_t *svc,
                                                                            const struct bt_uuid *uuid,
                                                                            uint8_t props_mask)
{
    if (!svc || !uuid || props_mask == 0)
        return NULL;
    sys_snode_t *char_node;
    SYS_SLIST_FOR_EACH_NODE(&svc->characteristics, char_node)
    {
        multiconn_characteristic_t *chara = CONTAINER_OF(char_node, multiconn_characteristic_t, node);
        // Check UUID and if *any* of the requested properties are set
        if (bt_uuid_cmp((const struct bt_uuid *)&chara->uuid, uuid) == 0 && (chara->properties & props_mask))
        {
            return chara;
        }
    }
    return NULL; // Not found
}

#ifdef CONFIG_MULTICONN_CLIENT_DISCOVER_DESCRIPTORS_ENABLE
multiconn_descriptor_t *multiconn_client_find_descriptor(multiconn_characteristic_t *chara,
                                                         const struct bt_uuid *uuid)
{
    if (!chara || !uuid)
        return NULL;
    sys_snode_t *desc_node;
    SYS_SLIST_FOR_EACH_NODE(&chara->descriptors, desc_node)
    {
        multiconn_descriptor_t *desc = CONTAINER_OF(desc_node, multiconn_descriptor_t, node);
        if (bt_uuid_cmp((const struct bt_uuid *)&desc->uuid, uuid) == 0)
        {
            return desc;
        }
    }
    return NULL; // Not found
}
#endif // CONFIG_MULTICONN_CLIENT_DISCOVER_DESCRIPTORS_ENABLE

static void mcc_work_handler(struct k_work *work)
{
    struct multiconn_conn_context *ctx =
        CONTAINER_OF(work, struct multiconn_conn_context, work);
    int err = 0;

    // --- Pre-checks ---
    if (!ctx || !ctx->in_use || ctx->conn == NULL ||
        ctx->state == MULTICONN_STATE_DISCONNECTED)
    {
        LOG_WRN("Work handler called for inactive/invalid context %p or conn %p.",
                (void *)ctx, (void *)ctx ? (void *)ctx->conn : NULL);
        return;
    }

    LOG_DBG("Work handler: conn %p, state: %d", (void *)ctx->conn, ctx->state);

    switch (ctx->state)
    {
        case MULTICONN_STATE_CONNECTED:
            // Start MTU Exchange
            ctx->state = MULTICONN_STATE_MTU_EXCHANGING;
            ctx->mtu_params.func = mcc_mtu_exchange_cb;
            err = bt_gatt_exchange_mtu(ctx->conn, &ctx->mtu_params);
            if (err)
            {
                LOG_ERR("Failed to start MTU exchange for conn %p (err %d)",
                        (void *)ctx->conn, err);
                ctx->last_err = err;
                ctx->state = MULTICONN_STATE_ERROR;
                schedule_next_action(ctx); // Schedule error handling
            }
            else
            {
                LOG_DBG("MTU exchange started for conn %p.", (void *)ctx->conn);
                // Wait for mcc_mtu_exchange_cb
            }
            break;

#ifdef CONFIG_MULTICONN_CLIENT_CONN_PARAM_UPDATE_ENABLE
        case MULTICONN_STATE_CONN_PARAM_UPDATING:
            // Request Connection Parameter Update
            err = bt_conn_le_param_update(ctx->conn, &ctx->desired_conn_params);
            if (err &&
                err !=
                    -EALREADY)
            { // EALREADY means request is pending, wait for callback
                LOG_ERR("Failed to request conn param update for conn %p (err %d)",
                        (void *)ctx->conn, err);
                // Treat as non-fatal? Log and continue to service discovery.
                LOG_WRN("Proceeding to service discovery despite conn param update "
                        "request failure.");
                ctx->state = MULTICONN_STATE_SERVICE_DISCOVERING;
                schedule_next_action(ctx);
            }
            else if (err == -EALREADY)
            {
                LOG_WRN("Connection parameter update already pending for conn %p.",
                        (void *)ctx->conn);
                // Wait for mcc_le_param_updated callback
            }
            else
            {
                LOG_DBG("Connection parameter update requested for conn %p.",
                        (void *)ctx->conn);
                // Wait for mcc_le_param_updated callback
            }
            break;
#endif

        case MULTICONN_STATE_SERVICE_DISCOVERING:
            // Start Primary Service Discovery
            ctx->discover_params.uuid = NULL; // Discover all primary
            ctx->discover_params.start_handle = BT_ATT_FIRST_ATTRIBUTE_HANDLE;
            ctx->discover_params.end_handle = BT_ATT_LAST_ATTRIBUTE_HANDLE;
            ctx->discover_params.type = BT_GATT_DISCOVER_PRIMARY;
            ctx->discover_params.func = mcc_discover_service_cb;
            err = bt_gatt_discover(ctx->conn, &ctx->discover_params);
            if (err)
            {
                LOG_ERR("Failed to start service discovery for conn %p (err %d)",
                        (void *)ctx->conn, err);
                ctx->last_err = err;
                ctx->state = MULTICONN_STATE_ERROR;
                schedule_next_action(ctx);
            }
            else
            {
                LOG_DBG("Service discovery started for conn %p.", (void *)ctx->conn);
                // Wait for mcc_discover_service_cb
            }
            break;

        case MULTICONN_STATE_CHAR_DISCOVERING:
            // Start Characteristic Discovery for the current service
            if (ctx->current_service_node == NULL)
            {
                // This means all services have been processed for char discovery
                LOG_INF("Characteristic discovery stage finished for conn %p.", (void *)ctx->conn);
#ifdef CONFIG_MULTICONN_CLIENT_DISCOVER_DESCRIPTORS_ENABLE
                if (ctx->service_count > 0)
                {
                    // Transition to Descriptor Discovery
                    ctx->state = MULTICONN_STATE_DESC_DISCOVERING;
                    // Reset iterators to the beginning for descriptor discovery
                    ctx->current_service_node = sys_slist_peek_head(&ctx->services);
                    multiconn_service_t *first_svc = CONTAINER_OF(ctx->current_service_node, multiconn_service_t, node);
                    ctx->current_char_node = sys_slist_peek_head(&first_svc->characteristics);
                    schedule_next_action(ctx); // Schedule descriptor discovery start
                }
                else
                {
                    // No services found, skip descriptor discovery
                    ctx->last_err = err;
                    ctx->state = MULTICONN_STATE_ERROR;
                    schedule_next_action(ctx);
                }
#else
                // Descriptor discovery disabled, check for auto-enable CCC
                // goto check_auto_enable_ccc_label;
                ctx->last_err = err;
                ctx->state = MULTICONN_STATE_ERROR;
                schedule_next_action(ctx);
#endif
            }
            else
            {
                // Start discovery for the current service pointed to by current_service_node
                multiconn_service_t *svc = CONTAINER_OF(ctx->current_service_node, multiconn_service_t, node);
                ctx->discover_params.uuid = NULL;                          // Discover all characteristics
                ctx->discover_params.start_handle = svc->start_handle + 1; // Start after service declaration
                ctx->discover_params.end_handle = svc->end_handle;
                ctx->discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC;
                ctx->discover_params.func = mcc_discover_characteristic_cb;

                // Ensure start handle is valid
                if (ctx->discover_params.start_handle > svc->end_handle)
                {
                    LOG_WRN("Service start+1 (0x%04x) > end (0x%04x) for conn %p, skipping char discovery.",
                            ctx->discover_params.start_handle, svc->end_handle, (void *)ctx->conn);
                    // Skip to next service
                    ctx->current_service_node = sys_slist_peek_next(ctx->current_service_node);
                    schedule_next_action(ctx); // Reschedule to handle next service or finish
                    break;                     // Exit current work handler execution
                }

                err = bt_gatt_discover(ctx->conn, &ctx->discover_params);
                if (err)
                {
                    LOG_ERR("Failed start char discovery (svc start 0x%04x) conn %p (err %d)",
                            svc->start_handle, (void *)ctx->conn, err);
                    ctx->last_err = err;
                    ctx->state = MULTICONN_STATE_ERROR;
                    schedule_next_action(ctx);
                }
                else
                {
                    char svc_uuid_str[BT_UUID_STR_LEN];
                    bt_uuid_to_str((const struct bt_uuid *)&svc->uuid, svc_uuid_str, sizeof(svc_uuid_str));
                    LOG_DBG("Char discovery started for service %s (0x%04x-0x%04x) conn %p.",
                            svc_uuid_str, ctx->discover_params.start_handle, ctx->discover_params.end_handle, (void *)ctx->conn);
                    // Wait for mcc_discover_characteristic_cb
                }
            }
            break;

#ifdef CONFIG_MULTICONN_CLIENT_DISCOVER_DESCRIPTORS_ENABLE
        case MULTICONN_STATE_DESC_DISCOVERING:
            {
                if (ctx->current_service_node == NULL)
                {
                    LOG_INF("Descriptor discovery stage finished for conn %p.", (void *)ctx->conn);
                    // If auto-enable wasn't triggered or isn't enabled, transition to READY
                    if (ctx->last_err == 0)
                    {
                        LOG_DBG("Transitioning state to READY after discovery stages.");
                        ctx->state = MULTICONN_STATE_READY;
                        schedule_next_action(ctx); // Schedule the final READY state handling
                    }
                    else
                    {
                        // If discovery ended with an error, ensure state is ERROR
                        LOG_DBG("Transitioning state to ERROR after discovery stages.");
                        ctx->state = MULTICONN_STATE_ERROR;
                        schedule_next_action(ctx); // Schedule error handling
                    }
                    return; // return ?
                }

                multiconn_service_t *current_svc = CONTAINER_OF(ctx->current_service_node, multiconn_service_t, node);

                // If current_char_node is NULL, find the first char in the current service
                if (ctx->current_char_node == NULL)
                {
                    ctx->current_char_node = sys_slist_peek_head(&current_svc->characteristics);
                    // If service has no characteristics, skip to next service
                    if (ctx->current_char_node == NULL)
                    {
                        char svc_uuid_str[BT_UUID_STR_LEN];
                        bt_uuid_to_str((const struct bt_uuid *)&current_svc->uuid, svc_uuid_str, sizeof(svc_uuid_str));
                        LOG_DBG("No characteristics in service UUID %s to discover descriptors for.", svc_uuid_str);
                        ctx->current_service_node = sys_slist_peek_next(ctx->current_service_node);
                        schedule_next_action(ctx);
                        return;
                    }
                }

                multiconn_characteristic_t *current_char = CONTAINER_OF(ctx->current_char_node, multiconn_characteristic_t, node);

                // Check if descriptors can exist after the value handle
                if (current_char->value_handle < current_svc->end_handle)
                {
                    // Start descriptor discovery for this characteristic
                    ctx->discover_params.uuid = NULL; // Discover all descriptors
                    ctx->discover_params.start_handle = current_char->value_handle + 1;
                    ctx->discover_params.end_handle = current_svc->end_handle;
                    ctx->discover_params.type = BT_GATT_DISCOVER_DESCRIPTOR;
                    ctx->discover_params.func = mcc_discover_descriptor_cb;

                    err = bt_gatt_discover(ctx->conn, &ctx->discover_params);
                    if (err)
                    {
                        LOG_ERR("Failed start desc discovery (char value 0x%04x) conn %p (err %d)",
                                current_char->value_handle, (void *)ctx->conn, err);
                        ctx->last_err = err;
                        ctx->state = MULTICONN_STATE_ERROR;
                        schedule_next_action(ctx); // Schedule error handling
                        return;                    // Exit work handler
                    }
                    else
                    {
                        char char_uuid_str[BT_UUID_STR_LEN];
                        bt_uuid_to_str((const struct bt_uuid *)&current_char->uuid, char_uuid_str, sizeof(char_uuid_str));
                        LOG_DBG("Desc discovery started for char %s (0x%04x-0x%04x) conn %p.",
                                char_uuid_str, ctx->discover_params.start_handle, ctx->discover_params.end_handle, (void *)ctx->conn);
                        return; // Exit work handler, wait for mcc_discover_descriptor_cb
                    }
                }
                else
                {
                    // No space for descriptors, skip this characteristic
                    LOG_DBG("Skipping desc discovery for char (value 0x%04x), no space after value.",
                            current_char->value_handle);
                    ctx->current_char_node = sys_slist_peek_next(ctx->current_char_node);

                    if (ctx->current_char_node == NULL)
                    {
                        ctx->current_service_node = sys_slist_peek_next(ctx->current_service_node);
                    }
                    schedule_next_action(ctx);
                    return;
                }
            }
            break;
#endif // CONFIG_MULTICONN_CLIENT_DISCOVER_DESCRIPTORS_ENABLE

        case MULTICONN_STATE_READY:
            // Final state, initialization successful. Notify application.
            LOG_INF("Initialization complete, conn %p is READY.", (void *)ctx->conn);
            if (app_callbacks && app_callbacks->ready)
            {
                app_callbacks->ready(ctx, 0); // 0 indicates success
            }
            // No further actions scheduled by the library for this context.
            break;

        case MULTICONN_STATE_ERROR:
            // Final state, initialization failed. Notify application.
            LOG_ERR("Initialization failed, conn %p in ERROR state (last_err %d).",
                    (void *)ctx->conn, ctx->last_err);
            if (app_callbacks && app_callbacks->ready)
            {
                app_callbacks->ready(ctx, ctx->last_err); // Pass the error code
            }
            // No further actions scheduled. Application might want to disconnect.
            break;

        default:
            LOG_WRN("Unhandled state %d in work handler for conn %p", ctx->state,
                    (void *)ctx->conn);
            break;
    }

    // if ((ctx->state == MULTICONN_STATE_CHAR_DISCOVERING && !IS_ENABLED(CONFIG_MULTICONN_CLIENT_DISCOVER_DESCRIPTORS_ENABLE)) ||
    //     (ctx->state == MULTICONN_STATE_DESC_DISCOVERING))
    // if(ctx->state == MULTICONN_STATE_DESC_DISCOVERING))
    // {
    //     // If auto-enable wasn't triggered or isn't enabled, transition to READY
    //     if (ctx->last_err == 0)
    //     {
    //         LOG_DBG("Transitioning state to READY after discovery stages.");
    //         ctx->state = MULTICONN_STATE_READY;
    //         schedule_next_action(ctx); // Schedule the final READY state handling
    //     }
    //     else
    //     {
    //         // If discovery ended with an error, ensure state is ERROR
    //         LOG_DBG("Transitioning state to ERROR after discovery stages.");
    //         ctx->state = MULTICONN_STATE_ERROR;
    //         schedule_next_action(ctx); // Schedule error handling
    //     }
    // }
}
